import numpy as np

class PathSmoothnessEvaluator:
    def __init__(self, heading_angles, unit='rad'):
        """
        初始化路径平滑性评估器

        参数：
            heading_angles : list 或 ndarray，航向角序列
            unit : 'rad' 或 'deg'，指定角度单位（默认是弧度）
        """
        self.unit = unit
        self.heading_angles = np.asarray(heading_angles)
        self._normalize_heading()

    def _normalize_heading(self):
        """将角度单位统一转换为弧度，便于计算"""
        if self.unit == 'deg':
            self.heading_angles = np.deg2rad(self.heading_angles)

    def compute_turning_angles(self):
        """计算相邻航向角的变化（标准化到 [-π, π]）"""
        turning_angles = np.diff(self.heading_angles)
        turning_angles = (turning_angles + np.pi) % (2 * np.pi) - np.pi
        return turning_angles

    def total_turning_angle(self):
        """返回累计转角"""
        turning_angles = self.compute_turning_angles()
        total = np.sum(np.abs(turning_angles))
        if self.unit == 'deg':
            total = np.rad2deg(total)
        return total

    def max_turning_angle(self):
        """返回最大单次转角"""
        turning_angles = self.compute_turning_angles()
        max_angle = np.max(np.abs(turning_angles))
        if self.unit == 'deg':
            max_angle = np.rad2deg(max_angle)
        return max_angle

    def evaluate(self):
        """返回一个字典，包括累计转角和最大转角"""
        return {
            'total_turning_angle': self.total_turning_angle(),
            'max_turning_angle': self.max_turning_angle()
        }
